/**
* @file BumperData.h
* @author
* @brief 碰撞传感器数据类型
* @version 1.0
* @date 2020-07-23
*
* @copyright Copyright (c) 2020
*
*/

#pragma once

#include <cstdint>
#include "lib/Time.h"
#include <map>

namespace behavior_controller
{

enum BumperId
{
    LEFT = 1, //左边bumper
    RIGHT = 2  //右边bumper
};


class BumperData
{
public:
    BumperData() {};

    BumperData(uint8_t id) : id_(id) {};

    /**
     * @brief 设置Bit
     * @return
     */
    void SetBit(uint8_t id)
    {
        id_ = id_ & id;
        real_time = Time::Now();
        if (id != 0)
        {
            time = real_time;
        }
    }

    void SetAllData(uint8_t id1, uint8_t id2)
    {
        data_array[0] = id1;
        data_array[1] = id2;
        real_time = Time::Now();
        if (id1 != 0 || id2 != 0)
        {
            time = real_time;
        }

    }

    void SetData(int i, uint8_t data)
    {
        data_array[i] = data;
        real_time = Time::Now();
        if (data != 0)
        {
            time = real_time;
        }
    }

    /**
     * @brief 获取二进制位
     * @return
     */
    uint8_t GetBit() const { return data_array[0] + data_array[1]; }

    /**
     * @brief 获取ID
     * @return
     */
    BumperId
    GetId() const
    {
        if (id_ & BumperId::LEFT)
        {
            return BumperId::LEFT;
        } else
        {
            return BumperId::RIGHT;
        }
    }

    double GetTime()
    { return time; }

    double GetRealTime()
    { return real_time; }

private:
    double time = 0;
    double real_time = 0;
    uint8_t id_; //Bumper的位数据
    uint8_t data_array[2] = {0};
public:
    std::map<std::string, int> bumper_map = {{std::string("base_contact_left"),  0},
                                             {std::string("base_contact_right"), 1}};
};

} // namespace behavior_controller
